#include "Precompile.h"
#include "QuaternionCamera.h"

namespace Utility{
	QuaternionCamera::QuaternionCamera(){
		m_pitch = 0;
		m_roll = 0;
		m_yaw = 0;

		// Cap pitch at 90C
		m_pitchCap = E_PI*0.5;
		m_yawCap = E_PI*2.0;
		m_rollCap = E_PI*2.0;

		m_glideFactor = 50;
		m_maxVelocity = 1.0f;
		m_updateTolerance = 0.0000000001f;

		m_pitchUpdated = true;
		m_rollUpdated = true;
		m_yawUpdated = true;

		m_viewNeedsUpdate = true;
	}

	void QuaternionCamera::update(float dt){
		// Decellerate
		m_velocity *= std::min(dt * m_glideFactor,0.9f);
	}

	void QuaternionCamera::roll(float radians){
		if(abs(radians) < m_updateTolerance)
			return;

		m_roll += radians;
		if(m_roll > 0 && m_roll >= m_rollCap)
			m_roll -= m_rollCap;
		else if(m_roll < 0 && m_roll <= -m_rollCap)
			m_roll += m_rollCap;

		m_rollUpdated = true;
		m_viewNeedsUpdate = true;
	}

	void QuaternionCamera::pitch(float radians){
		if(abs(radians) < m_updateTolerance)
			return;

		m_pitch += radians;
		if(m_pitch > 0 && m_pitch >= m_pitchCap)
			m_pitch = m_pitchCap;
		else if(m_pitch < 0 && m_pitch <= -m_pitchCap)
			m_pitch = -m_pitchCap;

		m_pitchUpdated = true;
		m_viewNeedsUpdate = true;
	}

	void QuaternionCamera::yaw(float radians){
		if(abs(radians) < m_updateTolerance)
			return;

		m_yaw += radians;
		if(m_yaw > 0 && m_yaw >= m_yawCap)
			m_yaw -= m_yawCap;
		else if(m_yaw < 0 && m_yaw <= -m_yawCap)
			m_yaw += m_yawCap;

		m_yawUpdated = true;
		m_viewNeedsUpdate = true;
	}

	void QuaternionCamera::moveForward(float units){
		if(abs(units) < m_updateTolerance)
			return;

		m_velocity += m_front*units;
		m_viewNeedsUpdate = true;
	}

	void QuaternionCamera::moveUp(float units){
		if(abs(units) < m_updateTolerance)
			return;

		m_velocity += m_up*units;
		m_viewNeedsUpdate = true;
	}
	
	void QuaternionCamera::strafe(float units){
		if(abs(units) < m_updateTolerance)
			return;

		m_velocity += m_right*units;
		m_viewNeedsUpdate = true;
	}

	Math::Matrix<4,4>& QuaternionCamera::getViewProjection(){
		if(m_viewNeedsUpdate){
			updateViewMatrix();
			m_viewProjMatrix = m_viewMatrix * m_projectionMatrix;
		}

		return ICamera::getViewProjection();
	}

	Math::Matrix<4,4>& QuaternionCamera::getViewMatrix(){
		if(m_viewNeedsUpdate){
			updateViewMatrix();
			m_viewProjMatrix = m_viewMatrix * m_projectionMatrix;
		}

		return ICamera::getViewMatrix();
	}

	void QuaternionCamera::updateViewMatrix(){
		if(m_yawUpdated){
			m_yawQuat.vec(Math::_y_,sinf(m_yaw/2.0f));
			m_yawQuat.vec(Math::_w_,cosf(m_yaw/2.0f));
			m_yawUpdated = false;
		}

		if(m_pitchUpdated){
			m_pitchQuat.vec(Math::_x_,sinf(m_pitch/2.0f));
			m_pitchQuat.vec(Math::_w_,cosf(m_pitch/2.0f));
			m_pitchUpdated = false;
		}

		if(m_rollUpdated){
			m_rollQuat.vec(Math::_z_,sinf(m_roll/2.0f));
			m_rollQuat.vec(Math::_w_,cosf(m_roll/2.0f));
			m_rollUpdated = false;
		}

		// Compute rotation
		m_orientation = m_yawQuat * m_pitchQuat * m_rollQuat;
		m_viewMatrix.createFromQuaternion(m_orientation);

		// Cap velocity
		if(m_velocity.length() > m_maxVelocity){
			m_velocity.normalize();
			m_velocity *= m_maxVelocity;
		}
		m_position += m_velocity;

		// update view translation
		m_front(Math::_x_,m_viewMatrix.get(0,2));
		m_front(Math::_y_,m_viewMatrix.get(1,2));
		m_front(Math::_z_,m_viewMatrix.get(2,2));

		m_up(Math::_x_,m_viewMatrix.get(0,1));
		m_up(Math::_y_,m_viewMatrix.get(1,1));
		m_up(Math::_z_,m_viewMatrix.get(2,1));

		m_right(Math::_x_,m_viewMatrix.get(0,0));
		m_right(Math::_y_,m_viewMatrix.get(1,0));
		m_right(Math::_z_,m_viewMatrix.get(2,0));

		m_viewMatrix.set(3,2,(-m_front.dot(m_position)));
		m_viewMatrix.set(3,1,(-m_up.dot(m_position)));
		m_viewMatrix.set(3,0,(-m_right.dot(m_position)));

		// reset update flag
		m_viewNeedsUpdate = false;
	}
}